// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ObjectDetectionTest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------

namespace Test.Robotics.Vision
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using System.Threading;
    using System.Windows;

    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.PhysicalObjects;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision;
    using Microsoft.Robotics.Vision.Cameras;
    using Microsoft.Robotics.Vision.ObjectDetection;
    using Microsoft.Robotics.Vision.Runtime;
    using Microsoft.Robotics.Vision.Runtime.Cameras;
    using Microsoft.VisualStudio.TestTools.UnitTesting;
    using Test.Robotics.Runtime;

    /// <summary>
    /// Test of object detection
    /// </summary>
    [TestClass]
    [DeploymentItem(@"Data\ObjectDetection\Configurations\tomatoCanDetector_v3.xml", @"Data\ObjectDetection\Configurations")]
    [DeploymentItem(@"ObjectDetection\ObjectDetectionTestImage_2014-01-22T08_10_38.bin", @"ObjectDetection")]
    [DeploymentItem(@"nnRunTimeManaged.dll")]
    [DeploymentItem(@"Microsoft.Robotics.Vision.Native.dll")]
    public class ObjectDetectionTest
    {
        /// <summary>
        /// Detector configuration file
        /// </summary>
        private const string ConfigFile = @"Data\ObjectDetection\Configurations\tomatoCanDetector_v3.xml";

        /// <summary>
        /// Test image file
        /// </summary>
        private const string ImageFile = @"ObjectDetection\ObjectDetectionTestImage_2014-01-22T08_10_38.bin";

        /// <summary>
        /// Test object detection level 1 classification
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ObjectDetectionLevel1Test()
        {
            KinectIRObjectDetectorConfiguration config = KinectIRObjectDetectorConfiguration.Create(ObjectDetectionTest.ConfigFile);
            ObjectPosePredictor predictor = ObjectDetectionTest.CreateTestPredictor(config);

            ObjectDetectionFeaturization featurizer = ObjectDetectionTest.CreateTestFeaturizer(config);

            int col = 339;
            int row = 254;
            double depth = featurizer.DepthFrame[col, row] / 1000.0;
            PhysicalObjectDescription manipObject = config.ManipulatedObjectDescription;

            int objectWidthMMScaled = (int)(config.ObjectWidthMM * config.PatchContextScale);
            int objectHeightMMScaled = (int)(config.ObjectHeightMM * config.PatchContextScale);
            KinectCameraCalibration cameraCalibration = ObjectDetectionTest.CreateCameraCalibration();

            double patchWidthPix;
            double patchHeightPix;
            double patchDepthPix;
            cameraCalibration.GetPatchSizeInPixForObject(col, row, depth, manipObject.WidthInM, manipObject.HeightInM, manipObject.DepthInM, config.PatchContextScale, out patchWidthPix, out patchHeightPix, out patchDepthPix);

            Assert.AreEqual((int)patchWidthPix, 38);
            Assert.AreEqual((int)patchHeightPix, 47);

            int[] feats = featurizer.GetIntegralFeatures(col, row, (int)patchWidthPix, (int)patchHeightPix);
            Assert.AreEqual(feats.Length, 15);

            double result = predictor.ClassifyPatchL1(
                                            featurizer,
                                            col,
                                            row,
                                            (int)patchWidthPix,
                                            (int)patchHeightPix);

            Assert.AreEqual(result, 0.95, 0.1);
        }

        /// <summary>
        /// Test object detection level 2 classification and regression
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ObjectDetectionLevel2Test()
        {
            KinectIRObjectDetectorConfiguration config = KinectIRObjectDetectorConfiguration.Create(ObjectDetectionTest.ConfigFile);
            ObjectPosePredictor predictor = ObjectDetectionTest.CreateTestPredictor(config);

            ObjectDetectionFeaturization featurizer = ObjectDetectionTest.CreateTestFeaturizer(config);

            int col = 339;
            int row = 254;
            double patchWidthPix;
            double patchHeightPix;
            double patchDepthMM;
            double depthM = featurizer.DepthFrame[col, row] / MathConstants.MilliunitsPerUnit;
            PhysicalObjectDescription manipObject = config.ManipulatedObjectDescription;
            KinectCameraCalibration cameraCalibration = ObjectDetectionTest.CreateCameraCalibration();
            cameraCalibration.GetPatchSizeInPixForObject(col, row, depthM, manipObject.WidthInM, manipObject.HeightInM, manipObject.DepthInM, config.PatchContextScale, out patchWidthPix, out patchHeightPix, out patchDepthMM);

            List<float> result = predictor.ClassifyPatchL2(
                                            featurizer,
                                            col,
                                            row,
                                            (int)patchWidthPix,
                                            (int)patchHeightPix,
                                            config.Level2Threshold, 
                                            2500);

            Assert.AreEqual(result.Count, 4);
            double centerX = (double)(col + patchWidthPix * (result[1] - 0.5));
            double centerY = (double)(row + patchHeightPix * (result[2] - 0.5));
            double centerZ  = depthM + patchDepthMM * (result[3] - 0.5);

            Assert.AreEqual(result[0], 1.0, 0.1);
            Assert.AreEqual(centerX, 336, 5);
            Assert.AreEqual(centerY, 250, 5);
            Assert.AreEqual(centerZ, 1.151, 0.01);
        }

        /// <summary>
        /// Test object detection level 2 classification fail case
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ObjectDetectionLevel2TestFail()
        {
            KinectIRObjectDetectorConfiguration config = KinectIRObjectDetectorConfiguration.Create(ObjectDetectionTest.ConfigFile);
            ObjectPosePredictor predictor = ObjectDetectionTest.CreateTestPredictor(config);

            ObjectDetectionFeaturization featurizer = ObjectDetectionTest.CreateTestFeaturizer(config);

            int col = 10;
            int row = 237;
            int patchWidth = 26;
            int patchHeight = 40;

            List<float> result = predictor.ClassifyPatchL2(
                                            featurizer,
                                            col,
                                            row,
                                            patchWidth,
                                            patchHeight,
                                            config.Level2Threshold,
                                            2500);

            Assert.IsNull(result);
        }

        /// <summary>
        /// Test object detection level 2 classification fail case
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void ObjectDetectionLevel2TestException()
        {
            KinectIRObjectDetectorConfiguration config = KinectIRObjectDetectorConfiguration.Create(ObjectDetectionTest.ConfigFile);
            KinectIRObjectDetectorConfiguration config1 = new KinectIRObjectDetectorConfiguration(
                    config.ManipulatedObjectDescription,
                    config.Level1Classifier,
                    config.Level2Classifier,
                    config.Level2Regress,
                    config.Level2OrientationRegress,
                    config.ConfidenceEstimator,
                    new Size(0, 0),
                    config.Level1Threshold,
                    config.Level2Threshold,
                    config.ConfidenceThreshold,
                    config.PatchContextScale,
                    config.DepthNormalizationFactor,
                    config.NoReadingDepthValue);

            ObjectPosePredictor predictor = ObjectDetectionTest.CreateTestPredictor(config1);
            ObjectDetectionFeaturization featurizer = ObjectDetectionTest.CreateTestFeaturizer(config1);

            int col = 330;
            int row = 237;
            int patchWidth = 26;
            int patchHeight = 40;

            List<float> result = predictor.ClassifyPatchL2(
                                            featurizer,
                                            col,
                                            row,
                                            patchWidth,
                                            patchHeight,
                                            config.Level2Threshold,
                                            2500);
        }

        /// <summary>
        /// Test top level object detection failure case when
        /// image frame is null
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void ObjectDetectionNullFrameException()
        {
            ObjectDetectionFeaturization featurizer = new ObjectDetectionFeaturization(null, null, 22.0, 34.0);
        }

        /// <summary>
        /// Test top level object detection failure case when
        /// there is a size mismatch between the IR and depth frames
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        [ExpectedException(typeof(ArgumentException))]
        public void ObjectDetectionFrameMismatchException()
        {
            KinectIRObjectDetectorConfiguration config = KinectIRObjectDetectorConfiguration.Create(ObjectDetectionTest.ConfigFile);
            ObjectPosePredictor predictor = ObjectDetectionTest.CreateTestPredictor(config);
            KinectCameraCalibration cameraCal = ObjectDetectionTest.CreateCameraCalibration();
            ImageFrame<ushort> ir = new ImageFrame<ushort>(10, 10);
            ImageFrameDepth depth = new ImageFrameDepth(20, 10);
            ObjectDetectionFeaturization featurizer = new ObjectDetectionFeaturization(
                    ir,
                    depth,
                    cameraCal.FocalX,
                    cameraCal.FocalY);

            predictor.Predict(featurizer, cameraCal);
        }

        /// <summary>
        /// Sets up a and pumps a recorded
        /// image sequence through a visual integration pipeline
        /// </summary>
        [Priority(1)]
        [TestMethod]
        [TestCategory("Unit")]
        public void ObjectDetectionAgentTest()
        {
            string filePath = @"\\robofile\Baker\Shared\Users\roboim\UnitTestData\Vision\ObjectDetectionAgentTest_V3.xml";

            // Set up a simple pipeline to read the data file
            AgentLocator agentLocator = new AgentLocator(true);
            TimerAgent timer = new TimerAgent("FileTimer", 1);
            agentLocator.Register(timer);
            double[,] mat = new double[,] { { 584.037, 0.0, 325.914 }, { 0, 583.505, 236.022 }, { 0, 0, 1 } };

            FilePlayerProxy player = new FilePlayerProxy(
                name: "player",
                filePath: filePath,
                messageTypes: new[] { typeof(DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters>) },
                timerAgent: timer,
                binarySerialization: false,
                skipAheadToFirstMessage: true);

            agentLocator.Register(player);

            KinectObjectDetectorIRAgent objectDetector = new KinectObjectDetectorIRAgent(
                            name: "objectDetector",
                            producer: player.AsProducer<DepthIRColorCameraMessage<DepthParameters, IRParameters, ColorParameters>>(),
                            detectorConfigurationFiles: new string[1] { ObjectDetectionTest.ConfigFile });
            agentLocator.Register(objectDetector);

            int expectedMessageCount = 1;
            MessageCollectorAgent<KinectObjectDetectorResultIRMessage> collectorAgent =
                new MessageCollectorAgent<KinectObjectDetectorResultIRMessage>("collector", objectDetector, expectedMessageCount);
            IAgentHost collector = agentLocator.Register(collectorAgent);

            try
            {
                collector.Activate();

                DateTime start = DateTime.Now;

                while (collectorAgent.Results.Count <= 0 && (DateTime.Now - start).TotalMilliseconds < 15000)
                {
                    Thread.Sleep(10);
                }

                Assert.IsTrue(collectorAgent.Results.Count > 0, "Expected at least 1 detection result");
                KinectObjectDetectorResultIRMessage result = collectorAgent.Results.Dequeue();
                Assert.AreEqual(result.State.Length, 1);
                ObjectPoseEstimate est = result.State[0];
                Assert.AreEqual(est.GlobalPose.Position.X, -0.560, 0.01);
                Assert.AreEqual(est.GlobalPose.Position.Y, -0.771, 0.01);
                Assert.AreEqual(est.GlobalPose.Position.Z, 0.036, 0.01);
                Assert.IsTrue(est.Confidence >= 0.80);
                Assert.IsTrue(0 == string.Compare(est.ManipulatedObjectDescription.Description, "tomatocan", true));
            }
            catch (Exception)
            {
                throw;
            }
            finally
            {
                collector.Deactivate();
            }
        }

        /// <summary>
        /// Create a test predictor
        /// </summary>
        /// <param name="config">Detector configuration</param>
        /// <returns>Created test predictor</returns>        
        private static ObjectPosePredictor CreateTestPredictor(KinectIRObjectDetectorConfiguration config)
        {
            ObjectPosePredictor predictor = new ObjectPosePredictor(config);
            return predictor;
        }

        /// <summary>
        /// Create a test featurizer
        /// </summary>
        /// <param name="config">Detector configuration</param>
        /// <returns>Test featurizer</returns>
        private static ObjectDetectionFeaturization CreateTestFeaturizer(KinectIRObjectDetectorConfiguration config)
        {
            ObjectPosePredictor predictor = new ObjectPosePredictor(config);
            ImageFrame<ushort> irFrame;
            ImageFrameDepth depthFrame;
            KinectCameraCalibration cameraCal = ObjectDetectionTest.CreateCameraCalibration();

            ObjectDetectionTest.LoadFrames(ObjectDetectionTest.ImageFile, config.NoReadingDepthValue, out irFrame, out depthFrame);
            ObjectDetectionFeaturization featurizer = new ObjectDetectionFeaturization(
                    irFrame,
                    depthFrame,
                    cameraCal.FocalX,
                    cameraCal.FocalY);

            return featurizer;
        }

        /// <summary>
        /// Create a test camera 
        /// </summary>
        /// <returns>Created camera configuration</returns>
        private static KinectCameraCalibration CreateCameraCalibration()
        {
            double[,] mat = new double[,] { { -364.672, 0, 251.587 }, { 0, 364.448, 200.139 }, { 0, 0, 1 } };
            Pose cameraPose = new Pose(new Vector3(-0.693815009180412, 0.224472275485447, 1.56972987201411), new Quaternion(0.488832927639197, -0.489846974117408, 0.491134227887734, 0.529036370206086));

            return new KinectCameraCalibration(new Matrix3(mat), cameraPose);
        }

        /// <summary>
        /// Load image frame from a test file
        /// </summary>
        /// <param name="imageFile">Image file</param>
        /// <param name="noReading">Kinect no reading value</param>
        /// <param name="irFrame">Loaded IR frame</param>
        /// <param name="depthFrame">Loaded depth frame</param>
        private static void LoadFrames(string imageFile, int noReading, out ImageFrame<ushort> irFrame, out ImageFrameDepth depthFrame)
        {
            using (FileStream fs = new FileStream(imageFile, FileMode.Open))
            {
                BinaryReader br = new BinaryReader(fs);
                ObjectDetectionTest.LoadFrame(br, noReading, out irFrame);
                ObjectDetectionTest.LoadFrame(br, noReading, out depthFrame);
            }
        }

        /// <summary>
        /// Load a single frame from a stream
        /// </summary>
        /// <param name="br">Stream reader</param>
        /// <param name="noReading">kinect no reading value</param>
        /// <param name="imageFrame">Loaded image frame</param>
        private static void LoadFrame(BinaryReader br, int noReading, out ImageFrame<ushort> imageFrame)
        {
            int width = br.ReadInt32();
            int height = br.ReadInt32();
            int bytesPerPixel = br.ReadInt32();

            imageFrame = new ImageFrame<ushort>(width, height);

            char[] next = new char[bytesPerPixel];

            int idx = 0;
            for (int row = 0; row < height; ++row)
            {
                for (int col = 0; col < width; ++col, idx += bytesPerPixel)
                {
                    int val = br.ReadInt16();
                    if (val <= 0)
                    {
                        val = noReading;
                    }

                    if (val > ushort.MaxValue)
                    {
                        val = 0;
                    }

                    imageFrame[col, row] = (ushort)val;
                }
            }
        }

        /// <summary>
        /// Load a single frame from a stream
        /// </summary>
        /// <param name="br">Stream reader</param>
        /// <param name="noReading">kinect no reading value</param>
        /// <param name="imageFrame">Loaded image frame</param>
        private static void LoadFrame(BinaryReader br, int noReading, out ImageFrameDepth imageFrame)
        {
            int width = br.ReadInt32();
            int height = br.ReadInt32();
            int bytesPerPixel = br.ReadInt32();

            imageFrame = new ImageFrameDepth(width, height);

            char[] next = new char[bytesPerPixel];

            int idx = 0;
            for (int row = 0; row < height; ++row)
            {
                for (int col = 0; col < width; ++col, idx += bytesPerPixel)
                {
                    int val = br.ReadInt16();
                    if (val <= 0)
                    {
                        val = noReading;
                    }

                    if (val > ushort.MaxValue)
                    {
                        val = 0;
                    }

                    imageFrame[col, row] = (short)val;
                }
            }
        }
    }
}
